package edu.umd.ion.wavemeter.server;

import org.nfunk.jep.JEP;

import edu.umd.ion.wavemeter.server.PIDControl.ChannelVoltage;
import edu.umd.ion.wavemeter.server.jep.Doublestep;
import edu.umd.ion.wavemeter.server.jep.DoublestepRamp;
import edu.umd.ion.wavemeter.server.jep.Rectangle;
import edu.umd.ion.wavemeter.server.jep.Triangle;
import edu.umd.ion.wavemeter.server.jep.Triplestep;
import edu.umd.ion.wavemeter.server.jep.TriplestepRamp;
import edu.umd.ion.wavemeter.service.PIDSettings;
import edu.umd.ion.wavemeter.service.WavemeterException;

public class DynamicPIDChannelData {
	private int Channel;
	private double P, I, D;
	private double Integral, LastValue, CurrentOutput, LastOutput, LastSetPoint;
	private boolean AutoClear;
	private boolean Enabled;
	private RunnableTwo<ChannelVoltage, Double> VoltageCallback;  // Callback to report current setting
	private JEP Course;                             // time in s
	private DynamicPIDChannelData[] AllChannelData;
	private String CourseString;
	private long TimeOffset, LastMeasurementTime, SystemTimeEnabled;  // system time in ms
	// private double MinOutput, MaxOutput;  // V
	private double Sensitivity = 100000;   // nm/mV 
	private double Polarity = 1;
	private final int NumVirtChannels = WavemeterConfiguration.NumVirtChannels();
	
	public DynamicPIDChannelData(int channel, DynamicPIDChannelData[] data) {
		AllChannelData = data;
		Channel = channel;
		Course = new JEP();
		Course.addStandardFunctions();
		Course.addStandardConstants();
		Course.addFunction("triangle", new Triangle() );
		Course.addFunction("rectangle", new Rectangle() );
		Course.addFunction("triplestep", new Triplestep() );
		Course.addFunction("triplestepramp", new TriplestepRamp() );
		Course.addFunction("doublestep", new Doublestep() );
		Course.addFunction("doublestepramp", new DoublestepRamp() );
		for (Integer i=new Integer(0); i<NumVirtChannels; ++i) {
			Course.addVariable("ch"+i.toString(),0);
		}		
		Course.addVariable("t", 0);
		Course.setImplicitMul(true);
		CourseString = "0";
		Course.parseExpression(CourseString);
	}
	
	public double getLastValue() {
		return LastValue;
	}
	
	public void enableRegulation(boolean enable, RunnableTwo<ChannelVoltage, Double> callback) {
		Enabled = enable;
		VoltageCallback = callback;
		if (AutoClear) { Integral = 0; }
	}
	
	public void clear() {
		Integral = 0;
	}
	
	/**
	 * Ensure that the double value is a number between -9999 and 9999
	 * and map +-inf and nan to 0
	 * @param val double val
	 * @return double in above limits
	 */
	public double confineValue(Double val) {
		if (val.isInfinite() || val.isNaN()) {
			val = 0.0;
		}
		return Math.min(Math.max(-9999, val), 9999);
	}
	
	public void measuredValue(long time, double result) {
		if (Enabled) {
			if (result>0) {
				LastSetPoint = getSetPoint(time);
				double delta_t = (time-LastMeasurementTime)/1000.;
				double delta = (result-LastSetPoint);
				Integral += delta*delta_t;
				double differential = (LastValue-result)/delta_t;
				CurrentOutput = confineValue( -Sensitivity*Polarity*(delta*P + Integral*I + differential*D) );
				LastMeasurementTime = time;
				LastValue = result;
			}
		} else { 
			if (AutoClear) { CurrentOutput = 0; }
			TimeOffset = LastMeasurementTime = time;
			LastValue = result;
			SystemTimeEnabled = System.currentTimeMillis();
		}
	}
	
	public void setCourse(String course) throws WavemeterException {
		Course.parseExpression(course);
		if (Course.hasError()) {
			throw new WavemeterException(Course.getErrorInfo());
		}
		CourseString = course;
	}
	
	public String getCourse() {
		return CourseString;
	}
	
	public double getSetPoint(long time) {
		double t = (time-TimeOffset)/1000;
		Course.setVarValue("t", t);
		for (Integer i=new Integer(0); i<NumVirtChannels; ++i) {
			Course.setVarValue("ch"+i.toString(),AllChannelData[i].getLastValue());
		}		
		return Course.getValue();
	}
	
	public boolean needsUpdate() {
		return LastOutput!=CurrentOutput;
	}
	
	public double getOutput() {
		if (VoltageCallback!=null) {
			ChannelVoltage cv = new ChannelVoltage();
			cv.Channel = Channel;
			cv.Voltage = CurrentOutput;
			VoltageCallback.run( cv, LastSetPoint );
		}
		return LastOutput = CurrentOutput;
	}
	
	public void setPID(PIDSettings PID) throws WavemeterException {
		P = PID.P;
		I = PID.I;
		D = PID.D;
		AutoClear = PID.AutoClearHistory;
		Polarity = PID.Polarity ? -1.0 : 1.0;
		if (PID.Course!=null && PID.Course.length()>0) {
			setCourse( PID.Course );
		}
	}

	public long getSystemTimeEnabled() {
		return SystemTimeEnabled;
	}

	public String getCourseString() {
		return CourseString;
	}
	
}
